#include "MyRobot.h"

/* UDP DOES NOT LIKE 1 BYTE PACKETS!!!
 * Packets come out as 'malformed' and add extra jargon at the end of what the
 * actual data is supposed to be. Two-byte and above packets work as they should.
 */

void RobotDemo::InitUDP()
{
	//Init Send
	int rv, rcvrv;
    memset(&hints, 0, sizeof hints);
    hints.ai_family = AF_INET;
    hints.ai_socktype = SOCK_DGRAM;
    
    
    if ((rv = getaddrinfo(ONBOARD_IP, SERVERPORT, &hints, &servinfo)) != 0)
    {
    	fprintf(stderr, "ERROR:getaddrinfo: %s\n", gai_strerror(rv));
        //printf("ERROR:getaddrinfo: %s\n", gai_strerror(rv));
        return;
    }
    
    // loop through all the results and make a socket
    for(p = servinfo; p != NULL; p = p->ai_next)
    {
        if ((sockfd = socket(p->ai_family, p->ai_socktype,
                p->ai_protocol)) == -1)
        {
        	//printf("ERROR:talker: socket\n");
            perror("talker: socket");
            continue;
        }
        break;
    }
    
    if (p == NULL)
    {
    	fprintf(stderr, "talker: failed to bind socket\n");
        //printf("ERROR:talker: failed to bind socket\n");
        return;
    }
    
    printf("Complete Send UDP init\n");
    
    //Init Receive
    memset(&rcvhints, 0, sizeof rcvhints);
    rcvhints.ai_family = AF_INET;
    rcvhints.ai_socktype = SOCK_DGRAM;
    rcvhints.ai_flags = AI_PASSIVE;
    
    if ((rcvrv = getaddrinfo(NULL, LISTENPORT, &rcvhints, &rcvservinfo)) != 0)
    {
        printf("ERROR:getaddrinfo: %s\n", gai_strerror(rcvrv));
        return;
    }
    
    // loop through all the results and bind to the first we can
    for(rcvp = rcvservinfo; rcvp != NULL; rcvp = rcvp->ai_next)
    {
        if ((rcvsockfd = socket(rcvp->ai_family, rcvp->ai_socktype,
                rcvp->ai_protocol)) == -1) {
            printf("ERROR:listener: socket\n");
        	//perror("listener: socket");
            continue;
        }

        if (bind(rcvsockfd, rcvp->ai_addr, rcvp->ai_addrlen) == -1)
        {
            close(rcvsockfd);
            printf("ERROR:listener: bind\n");
            //perror("listener: bind");
            continue;
        }

        break;
    }
    
    if (rcvp == NULL)
    {
        printf("ERROR:listener: failed to bind socket\n");
        return;
    }
    
    printf("Complete Rec UDP init\n");
}

void* RobotDemo::SendUDP(void* clsptr)
{
	RobotDemo* ptr = (RobotDemo*)clsptr;
	
	/*
	while(true)
	{
	    int numbytes;
	    char carray[10];
	    strcpy(carray, (ptr->displayShootFeed) ? FEED_SHOOT : FEED_PICKUP);
	    //strcpy(carray, "");
	    //char* c = (ptr->displayShootFeed) ? const_cast<char*>(FEED_SHOOT) : const_cast<char*>(FEED_PICKUP);
	    if ((numbytes = sendto(ptr->sockfd, carray, strlen(carray), 0,
	             ptr->p->ai_addr, ptr->p->ai_addrlen)) == -1)
	    {
	    	//printf("ERROR:talker: sendto; errno: %d\n", numbytes);
	    	perror("talker: sendto");
	        //return;
	    	//don't break the loop, allow it to finish, to hit the sleep
	    }
	
	    if (numbytes != -1) printf("UDP: sent %d bytes to %s\n", numbytes, SERVERPORT);
	    
	    sleep_ms(SLEEP_CYCLES);
	}
	*/
}

void* RobotDemo::RcvUDP(void* clsptr)
{
	RobotDemo* ptr = (RobotDemo*)clsptr;
	
	/*
	while(true)
	{
	    int numbytes;
	    char buf[RCVBUFFER];
	    socklen_t addr_len;
	    char s[INET_ADDRSTRLEN];
	
	    printf("listener: waiting to recvfrom...\n");
	
	    addr_len = sizeof ptr->their_addr;
	    if ((numbytes = recvfrom(ptr->rcvsockfd, buf, RCVBUFFER - 1 , 0,
	        (struct sockaddr *)&ptr->their_addr, (int*)&addr_len)) == -1)
	    {
	        perror("recvfrom");
	        //return;
	    }
	
	    printf("listener: got packet from %s\n",
	        inet_ntop(ptr->their_addr.ss_family,
			&(((struct sockaddr_in*)(struct sockaddr *)&ptr->their_addr)->sin_addr),
	        s, sizeof s));
	    printf("listener: packet is %d bytes long\n", numbytes);
	    buf[numbytes] = '\0';
	    printf("listener: packet contains \"%s\"\n", buf);
	    
	    ptr->rcvData = buf[0];
	    
	    sleep_ms(SLEEP_CYCLES);
	}
	*/
}
